import numpy as np
import math
from math import *

# 用于根据欧拉角计算旋转矩阵
def oula2Matrix(x, y, z):
    Rx = np.array([[1, 0, 0], [0, cos(x), -sin(x)], [0, sin(x), cos(x)]])
    Ry = np.array([[cos(y), 0, sin(y)], [0, 1, 0], [-sin(y), 0, cos(y)]])
    Rz = np.array([[cos(z), -sin(z), 0], [sin(z), cos(z), 0], [0, 0, 1]])
    R = np.dot(Rz, np.dot(Ry, Rx))
    return R

# 用于根据位姿计算变换矩阵
def toolsMatrix(Tx, Ty, Tz, x, y, z): 
    thetaX = x / 180 * pi
    thetaY = y / 180 * pi
    thetaZ = z / 180 * pi
    R = oula2Matrix(thetaX, thetaY, thetaZ)
    t = np.array([[Tx], [Ty], [Tz]])
    RT1 = np.column_stack([R, t])  # 列合并
    RT1 = np.row_stack((RT1, np.array([0, 0, 0, 1])))
    return RT1

# def rotateX(x):
#     Rx = np.array([[1, 0, 0], [0, cos(x), -sin(x)], [0, sin(x), cos(x)]])
#     Rx_homogeneous = np.eye(4)
#     Rx_homogeneous[:3, :3] = Rx
#     return Rx_homogeneous

# def rotateZ(z):
#     Rz = np.array([[cos(z), -sin(z), 0], [sin(z), cos(z), 0], [0, 0, 1]])
#     Rz_homogeneous = np.eye(4)
#     Rz_homogeneous[:3, :3] = Rz
#     return Rz_homogeneous

def rotateX(angle):
    Rx = np.array([
        [1, 0, 0, 0],
        [0, np.cos(angle), -np.sin(angle), 0],
        [0, np.sin(angle), np.cos(angle), 0],
        [0, 0, 0, 1]
    ])
    return Rx

def rotateZ(angle):
    Rz = np.array([
        [np.cos(angle), -np.sin(angle), 0, 0],
        [np.sin(angle), np.cos(angle), 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ])
    return Rz

def rotateY(angle):
    """返回绕Y轴旋转的4x4齐次变换矩阵"""
    Ry = np.array([
        [np.cos(angle), 0, np.sin(angle), 0],
        [0, 1, 0, 0],
        [-np.sin(angle), 0, np.cos(angle), 0],
        [0, 0, 0, 1]
    ])
    return Ry



def rotation_matrix_to_vector(R):
    # 确保R是3x3矩阵
    assert R.shape == (3, 3), "The input must be a 3x3 rotation matrix."
    
    # 计算旋转矩阵的迹
    trace = np.trace(R)
    
    # 计算旋转角度θ
    theta = np.arccos((trace - 1) / 2.0)
    
    # 如果θ接近0，则任何非零向量都可以作为旋转向量
    if np.isclose(theta, 0):
        # 可以选择任意非零向量，例如[1, 0, 0]
        w = np.array([1, 0, 0])
    else:
        # 计算旋转向量
        w = np.zeros(3)
        w[0] = R[2, 1] - R[1, 2]
        w[1] = R[0, 2] - R[2, 0]
        w[2] = R[1, 0] - R[0, 1]
        w = w / np.linalg.norm(w) * theta
    
    return w

# #旋转向量转旋转矩阵
# # 假设 rvecs[0] 是一个旋转向量
# rvecs = np.array([[0.1, 0.2, 0.3]])  # 示例旋转向量
# # 使用 cv2.Rodrigues 将旋转向量转换为旋转矩阵
# R, _ = cv2.Rodrigues(rvecs[0])


#假设的顺序是ZYX
def rotation_matrix_to_euler_angles(matrix):
    assert matrix.shape == (4, 4), "矩阵必须是 4x4 的"
    rotation_matrix = matrix[:3, :3]
    sy = np.sqrt(rotation_matrix[0, 0] * rotation_matrix[0, 0] + rotation_matrix[1, 0] * rotation_matrix[1, 0])
    singular = sy < 1e-6
    if not singular:
        x = np.arctan2(rotation_matrix[2, 1], rotation_matrix[2, 2])
        y = np.arctan2(-rotation_matrix[2, 0], sy)
        z = np.arctan2(rotation_matrix[1, 0], rotation_matrix[0, 0])
    else:
        x = np.arctan2(-rotation_matrix[1, 2], rotation_matrix[1, 1])
        y = np.arctan2(-rotation_matrix[2, 0], sy)
        z = 0
        
    x = x * 180 / np.pi
    y = y * 180 / np.pi
    z = z * 180 / np.pi
    return np.array([x, y, z])


#将[x,y,z,Rx,Ry,Rz]转换为matrix
def trans2matrix(targetpos):
    # 创建4x4的齐次变换矩阵
    T = np.eye(4)
    # 绕x轴的旋转矩阵
    Rx = math.radians(targetpos[3])
    Rx_matrix = np.array([
        [1, 0, 0],
        [0, np.cos(Rx), -np.sin(Rx)],
        [0, np.sin(Rx), np.cos(Rx)]
    ])
    
    # 绕y轴的旋转矩阵
    Ry = math.radians(targetpos[4])
    Ry_matrix = np.array([
        [np.cos(Ry), 0, np.sin(Ry)],
        [0, 1, 0],
        [-np.sin(Ry), 0, np.cos(Ry)]
    ])
    
    # 绕z轴的旋转矩阵
    Rz = math.radians(targetpos[5])
    Rz_matrix = np.array([
        [np.cos(Rz), -np.sin(Rz), 0],
        [np.sin(Rz), np.cos(Rz), 0],
        [0, 0, 1]
    ])
    R = Rz_matrix @ Ry_matrix @ Rx_matrix
    

    T[:3, :3] = R
    T[:3, 3] = [targetpos[0], targetpos[1], targetpos[2]]
    
    return T